#include "ethcatdevice.h"
#include "soem/soemethercat.h"
#include <vector>

void EthCATDevice::GetConstAttribut()
{
    std::lock_guard<std::mutex> lock(mtx);
    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::Name, sExchange);
    Name = sExchange;

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::OutputSize, sExchange);
    OutputSize = stoi(sExchange);
    if (OutputSize > 0)
        OutputData = new char[(OutputSize / 8) + (OutputSize % 8)];

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::InputSize, sExchange);
    InputSize = stoi(sExchange);
    if (InputSize > 0)
        InputData = new char[(InputSize / 8) + (InputSize % 8)];

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::Delay, sExchange);
    Delay = stoi(sExchange);

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::Config_Address, sExchange);
    Config_Address = stoi(sExchange);

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::ManufacturerId, sExchange);
    _ManufacturerId = stoi(sExchange);

    char _ManufacturerIdStr[10];
    sprintf(_ManufacturerIdStr, "%x", _ManufacturerId);
    ManufacturerId = "0x";
    ManufacturerId.append(_ManufacturerIdStr);

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::TypeId, sExchange);
    _TypeId = stoi(sExchange);
    char _TypeIdStr[10];
    sprintf(_TypeIdStr, "%x", _TypeId);
    TypeId = "0x";
    TypeId.append(_TypeIdStr);

    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::Rev, sExchange);
    Revision = stoi(sExchange);

    // Maibox Protocol
    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::MailboxProtocol, sExchange);
    unsigned short mbx = stoi(sExchange);

    MailboxProtocol.clear();
    std::vector<MailBoxProto> protos = {MailBoxProto::AoE, MailBoxProto::EoE, MailBoxProto::CoE
            ,MailBoxProto::FoE, MailBoxProto::SoE, MailBoxProto::VoE}; // 使用vector或其他容器存储枚举值
      for (const auto& proto : protos) {
          if (((mbx & ( unsigned short)(proto)) != 0)){
                  MailboxProtocol.append(std::to_string(proto)).append(" ");
          }
      }
}

void EthCATDevice::GetDynamicAttribut()
{
    std::lock_guard<std::mutex> lock(mtx);
    soemEtherCat_->GetDeviceInfo(Id, DeviceInfoParam::State, sExchange);
    State = (SlaveState)stoi(sExchange);
    soemEtherCat_->GetDeviceInfo(Id, (DeviceInfoParam)(-1), sExchange);
}

void EthCATDevice::Refresh()
{
    GetDynamicAttribut();
}

void EthCATDevice::WriteState(SlaveState st)
{
    Refresh();
    if (State != st)
    {
        std::lock_guard<std::mutex> lock(mtx);
        soemEtherCat_->WriteState((int)Id, st);
    }
}

bool EthCATDevice::CheckAndSetState() {
    Refresh();
    if (State != SlaveState::Operational)
    {
        // 设置启动状态
        soemEtherCat_->WriteState((int)Id, SlaveState::Init);
        Sleep(200);
        Refresh();
        soemEtherCat_->WriteState((int)Id, PreOperational);
        Sleep(200);
        Refresh();
        soemEtherCat_->WriteState((int)Id, Operational);
        Sleep(200);
        Refresh();
    }

    return State == Operational;
}

void EthCATDevice::Reconfigure()
{
    soemEtherCat_->Reconfigure(Id);
}

void EthCATDevice::WriteOutput()
{
    std::lock_guard<std::mutex> lock(mtx);
    {
        soemEtherCat_->SetOutput(Id, OutputData);
    }
}

int EthCATDevice::ReadInput()
{
    int n = 0;
    std::lock_guard<std::mutex> lock(mtx);
    n = soemEtherCat_->GetInput(Id, InputData);
    return n;
}

// SubIndex=-1 for all
char * EthCATDevice::ReadPDO(int Idx, int SubIndex, int size)
{
    char * buff = new char [4096];
    size = 4096;

    int ret = 0;
    std::lock_guard<std::mutex> lock(mtx);
    ret = soemEtherCat_->ReadPDO(Id, Idx, SubIndex, &size, buff);


    if (ret != 0)
    {
        printf("PDO Read OK");
        return buff;
    }
    else
    {
        printf("PDO Read Fail");
        return nullptr;
    }
}
// SubIndex=-1 for all
bool EthCATDevice::WritePDO(int Idx, int SubIndex, int size,char* buff)
{
    int ret = 0;
    std::lock_guard<std::mutex> lock(mtx);
    ret = soemEtherCat_->WritePDO(Id, Idx, SubIndex, size, buff);

    if (ret != 0)
    {
       printf("PDO Write OK");
        return true;
    }
    else
    {
        printf("PDO Write Fail");
        return false;
    }
}
